{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "# `ImuFactor` **vs.** `CombinedImuFactor`\n",
        "\n",
        "This notebook shows that the two ways of incorporating pre-integrated IMU data in GTSAM are *mathematically equivalent* **as long as the same continuous-time noise is used**:\n",
        "\n",
        "1. **`gtsam.ImuFactor` + `gtsam.BetweenFactorConstantBias`** – bias random walk injected afterwards with a BetweenFactor.\n",
        "2. **`gtsam.CombinedImuFactor`** – folds that BetweenFactor into the IMU factor itself.\n",
        "\n",
        "We build the two graphs side-by-side and show they produce identical posteriors."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "colab_button",
      "metadata": {},
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/Combined_vs_ImuFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "### Continuous-time *Q<sub>c</sub>* versus discrete-time *Σ*\n",
        "\n",
        "IMU bias drift is modelled as Brownian motion with power-spectral density (PSD)\n",
        "$$\\dot b(t)=w_b,\\qquad\\mathbb E[w_b w_b^T]=Q_c\\;(\\text{variance} \\cdot \\text{s}^{-1}).$$\n",
        "Over a finite interval $\\Delta t$ the discrete covariance is\n",
        "$$\\Sigma = Q_c\\,\\Delta t.$$\n",
        "\n",
        "* **`CombinedImuFactor`** – pass *continuous* $Q_c$ via `setBiasAccCovariance()` / `setBiasOmegaCovariance()`.  The implementation multiplies it by `dt` inside every `integrateMeasurement(dt)`, so the factor automatically accumulates $\\Sigma = Q_c\\,T$ after $T=\\sum dt$.\n",
        "* **`ImuFactor` + Between-factor** – do *not* add bias noise during pre-integration; instead attach a single `BetweenFactorConstantBias` spanning the same interval **T** and give it the *discrete* covariance $\\Sigma$ (implemented by scaling the standard deviation by $\\sqrt{T}$).\n",
        "\n",
        "Because both graphs end up with the **same Σ**, they contain the same information."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "> **Practical tip – when is `CombinedImuFactor` worth it?**\n",
        ">\n",
        "> IMU biases drift slowly (seconds to minutes).  If your key-frames are already that far apart, inserting one `BetweenFactorConstantBias` per key-frame is cheaper **and** accurate.  Reserve `CombinedImuFactor` for cases where you need bias estimates at *every* IMU integration interval."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "### Step 1  – Common setup and imports"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {},
      "outputs": [],
      "source": [
        "# Install gtsam-develop if not installed\n",
        "try:\n",
        "    import gtsam\n",
        "except ImportError:\n",
        "    %pip install --quiet gtsam-develop"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {},
      "outputs": [],
      "source": [
        "# Imports\n",
        "import gtsam\n",
        "import numpy as np\n",
        "from gtsam.symbol_shorthand import X, V, B"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {},
      "outputs": [],
      "source": [
        "def compare_matrices_side_by_side(mat1, mat2, precision=4):\n",
        "    mat1 = np.array(mat1)\n",
        "    mat2 = np.array(mat2)\n",
        "    rows = max(mat1.shape[0], mat2.shape[0])\n",
        "    m1 = np.array2string(mat1, precision=precision, suppress_small=True, max_line_width=80).splitlines()\n",
        "    m2 = np.array2string(mat2, precision=precision, suppress_small=True, max_line_width=80).splitlines()\n",
        "    for i in range(rows):\n",
        "        left  = m1[i] if i < len(m1) else \"\"\n",
        "        right = m2[i] if i < len(m2) else \"\"\n",
        "        print(f\"{left:<50} | {right}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "We simulate a **stationary** IMU: only gravity accelerates the sensor and the true angular velocity is zero."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {},
      "outputs": [],
      "source": [
        "# Scenario parameters\n",
        "dt               = 0.1    # 10 msps between states\n",
        "integration_time = 1.0    # each factor integrates 1 s (10×dt)\n",
        "gravity          = np.array([0, 0, -9.81])\n",
        "\n",
        "# IMU measurements (zero motion)\n",
        "measured_acc  = -gravity           # body-frame specific force\n",
        "measured_gyro = np.array([0, 0, 0])"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "**Pre-integration parameters.**\n",
        "`PreintegrationCombinedParams` additionally needs the bias PSDs (*continuous-time* $Q_c$); for the classic `ImuFactor` those are supplied later via the Between-factor."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {},
      "outputs": [],
      "source": [
        "# IMU sensor noise and bias random-walk PSDs (σ   = sqrt(Qc))\n",
        "accel_noise_sigma     = 1e-4\n",
        "gyro_noise_sigma      = 1e-4\n",
        "integration_noise_sigma = 1e-5\n",
        "bias_acc_walk_sigma   = 1e-4  #   σ  (m/s²)/√s\n",
        "bias_gyro_walk_sigma  = 1e-4  #   σ  (rad/s)/√s\n",
        "\n",
        "# Classic ImuFactor params\n",
        "params_imu = gtsam.PreintegrationParams(gravity)\n",
        "params_imu.setAccelerometerCovariance(np.eye(3)*accel_noise_sigma**2)\n",
        "params_imu.setGyroscopeCovariance    (np.eye(3)*gyro_noise_sigma**2)\n",
        "params_imu.setIntegrationCovariance  (np.eye(3)*integration_noise_sigma**2)\n",
        "\n",
        "# CombinedImuFactor params\n",
        "params_combined = gtsam.PreintegrationCombinedParams(gravity)\n",
        "params_combined.setAccelerometerCovariance(np.eye(3)*accel_noise_sigma**2)\n",
        "params_combined.setGyroscopeCovariance    (np.eye(3)*gyro_noise_sigma**2)\n",
        "params_combined.setIntegrationCovariance  (np.eye(3)*integration_noise_sigma**2)\n",
        "\n",
        "# ***Continuous-time*** bias PSDs:\n",
        "params_combined.setBiasAccCovariance (np.eye(3)*bias_acc_walk_sigma**2)\n",
        "params_combined.setBiasOmegaCovariance(np.eye(3)*bias_gyro_walk_sigma**2)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "### Step 2  – Pre-integration and covariance comparison\n",
        "\n",
        "Now we create the preintegrated measurement objects (`PIM`s) and integrate our simulated measurements. We can then directly compare their preintegrated measurement covariances.\n",
        "\n",
        "The classic PIM’s 9 × 9 covariance contains **only sensor noise**.  The combined PIM’s 15 × 15 covariance also contains the accumulated bias-random-walk noise: therefore its **top-left 9 × 9 block is *larger*** – exactly by $Q_c\\,T$ – and we explain later how the ImuFactor graph adds that same amount via the Between-factor."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "IMU-only 9×9  vs Combined 15×15 (×1e9):\n",
            "[[ 10.   0.   0.   0. -14.   0.   0. -44.   0.]    | [[ 13.   0.   0.   0. -16.   0.   0. -53.   0.   0.   0.   0.   5.   0.   0.]\n",
            " [  0.  10.   0.  14.   0.   0.  44.   0.   0.]    |  [  0.  13.   0.  16.   0.   0.  53.   0.   0.   0.   0.   0.   0.   5.   0.]\n",
            " [  0.   0.  10.   0.   0.   0.   0.   0.   0.]    |  [  0.   0.  13.   0.   0.   0.   0.   0.   0.   0.   0.   0.   0.   0.   5.]\n",
            " [  0.  14.   0.  40.   0.   0. 102.   0.   0.]    |  [  0.  16.   0.  42.   0.   0. 110.   0.   0.   1.   0.   0.   0.   3.   0.]\n",
            " [-14.   0.   0.   0.  40.   0.   0. 102.   0.]    |  [-16.   0.   0.   0.  42.   0.   0. 110.   0.   0.   1.   0.  -3.   0.   0.]\n",
            " [  0.   0.   0.   0.   0.   3.   0.   0.   5.]    |  [  0.   0.   0.   0.   0.   4.   0.   0.   6.   0.   0.   1.   0.   0.   0.]\n",
            " [  0.  44.   0. 102.   0.   0. 284.   0.   0.]    |  [  0.  53.   0. 110.   0.   0. 315.   0.   0.   5.   0.   0.   0.  12.   0.]\n",
            " [-44.   0.   0.   0. 102.   0.   0. 284.   0.]    |  [-53.   0.   0.   0. 110.   0.   0. 315.   0.   0.   5.   0. -12.   0.   0.]\n",
            " [  0.   0.   0.   0.   0.   5.   0.   0.  10.]]   |  [  0.   0.   0.   0.   0.   6.   0.   0.  13.   0.   0.   5.   0.   0.   0.]\n",
            "                                                   |  [  0.   0.   0.   1.   0.   0.   5.   0.   0.  10.   0.   0.   0.   0.   0.]\n",
            "                                                   |  [  0.   0.   0.   0.   1.   0.   0.   5.   0.   0.  10.   0.   0.   0.   0.]\n",
            "                                                   |  [  0.   0.   0.   0.   0.   1.   0.   0.   5.   0.   0.  10.   0.   0.   0.]\n",
            "                                                   |  [  5.   0.   0.   0.  -3.   0.   0. -12.   0.   0.   0.   0.  10.   0.   0.]\n",
            "                                                   |  [  0.   5.   0.   3.   0.   0.  12.   0.   0.   0.   0.   0.   0.  10.   0.]\n",
            "                                                   |  [  0.   0.   5.   0.   0.   0.   0.   0.   0.   0.   0.   0.   0.   0.  10.]]\n"
          ]
        }
      ],
      "source": [
        "# We start with a zero-bias assumption for preintegration\n",
        "bias_0 = gtsam.imuBias.ConstantBias()\n",
        "\n",
        "# Create the two PIM objects\n",
        "pim_imu      = gtsam.PreintegratedImuMeasurements     (params_imu,      bias_0)\n",
        "pim_combined = gtsam.PreintegratedCombinedMeasurements(params_combined, bias_0)\n",
        "\n",
        "# Integrate measurements\n",
        "for _ in range(int(integration_time/dt)):\n",
        "    pim_imu.integrateMeasurement     (measured_acc, measured_gyro, dt)\n",
        "    pim_combined.integrateMeasurement(measured_acc, measured_gyro, dt)\n",
        "\n",
        "print(\"IMU-only 9×9  vs Combined 15×15 (×1e9):\")\n",
        "compare_matrices_side_by_side(1e9*pim_imu.preintMeasCov(),\n",
        "                              1e9*pim_combined.preintMeasCov(), precision=0)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "### Step 3: Building Equivalent Factor Graphs\n",
        "Next, we construct the two factor graphs for a trajectory with two IMU integration steps (three states at times k=0, 1, 2). We will add identical priors to both graphs."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 7,
      "metadata": {},
      "outputs": [],
      "source": [
        "# Initial state at t=0\n",
        "pose_0 = gtsam.Pose3()\n",
        "vel_0 = np.zeros(3)\n",
        "# The bias_0 variable is the same as the one used for pre-integration above.\n",
        "\n",
        "# Define noise models for priors\n",
        "pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.01]*3 + [0.01]*3))\n",
        "velocity_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.01]*3))\n",
        "bias_prior_sigma = 0.01  # Standard deviation for bias prior\n",
        "bias_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([bias_prior_sigma]*6))\n",
        "\n",
        "# The bias random walk noise is defined once for the BetweenFactor\n",
        "bias_walk_noise_model = gtsam.noiseModel.Diagonal.Sigmas(\n",
        "    np.concatenate([np.full(3, bias_acc_walk_sigma), np.full(3, bias_gyro_walk_sigma)]) * np.sqrt(integration_time)\n",
        ")"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 8,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Graph 1 (ImuFactor + BetweenFactor):\n",
            "size: 7\n",
            "\n",
            "Factor 0: PriorFactor on x0\n",
            "  prior mean:  R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t: 0 0 0\n",
            "isotropic dim=6 sigma=0.01\n",
            "\n",
            "Factor 1: PriorFactor on v0\n",
            "  prior mean: [\n",
            "\t0;\n",
            "\t0;\n",
            "\t0\n",
            "]\n",
            "isotropic dim=3 sigma=0.01\n",
            "\n",
            "Factor 2: PriorFactor on b0\n",
            "  prior mean: acc = 0 0 0 gyro = 0 0 0\n",
            "isotropic dim=6 sigma=0.01\n",
            "\n",
            "Factor 3: \n",
            "ImuFactor(x0,v0,x1,v1,b0)\n",
            "preintegrated measurements:\n",
            "\n",
            "    deltaTij = 1\n",
            "    deltaRij.ypr = ( 0 -0  0)\n",
            "    deltaPij =     0     0 4.905\n",
            "    deltaVij =    0    0 9.81\n",
            "    gyrobias = 0 0 0\n",
            "    acc_bias = 0 0 0\n",
            "\n",
            "    preintMeasCov \n",
            "[       1e-08            0            0            0 -1.39793e-08            0            0  -4.4145e-08            0\n",
            "           0        1e-08            0  1.39793e-08            0            0   4.4145e-08            0            0\n",
            "           0            0        1e-08            0            0            0            0            0            0\n",
            "           0  1.39793e-08            0  4.03147e-08            0            0  1.02439e-07            0            0\n",
            "-1.39793e-08            0            0            0  4.03147e-08            0            0  1.02439e-07            0\n",
            "           0            0            0            0            0    3.425e-09            0            0        5e-09\n",
            "           0   4.4145e-08            0  1.02439e-07            0            0  2.84273e-07            0            0\n",
            " -4.4145e-08            0            0            0  1.02439e-07            0            0  2.84273e-07            0\n",
            "           0            0            0            0            0        5e-09            0            0        1e-08]\n",
            "  noise model sigmas:      0.0001      0.0001      0.0001 0.000200785 0.000200785 5.85235e-05 0.000533172 0.000533172      0.0001\n",
            "\n",
            "Factor 4: BetweenFactor(b0,b1)\n",
            "  measured: acc = 0 0 0 gyro = 0 0 0\n",
            "isotropic dim=6 sigma=0.0001\n",
            "\n",
            "Factor 5: \n",
            "ImuFactor(x1,v1,x2,v2,b1)\n",
            "preintegrated measurements:\n",
            "\n",
            "    deltaTij = 1\n",
            "    deltaRij.ypr = ( 0 -0  0)\n",
            "    deltaPij =     0     0 4.905\n",
            "    deltaVij =    0    0 9.81\n",
            "    gyrobias = 0 0 0\n",
            "    acc_bias = 0 0 0\n",
            "\n",
            "    preintMeasCov \n",
            "[       1e-08            0            0            0 -1.39793e-08            0            0  -4.4145e-08            0\n",
            "           0        1e-08            0  1.39793e-08            0            0   4.4145e-08            0            0\n",
            "           0            0        1e-08            0            0            0            0            0            0\n",
            "           0  1.39793e-08            0  4.03147e-08            0            0  1.02439e-07            0            0\n",
            "-1.39793e-08            0            0            0  4.03147e-08            0            0  1.02439e-07            0\n",
            "           0            0            0            0            0    3.425e-09            0            0        5e-09\n",
            "           0   4.4145e-08            0  1.02439e-07            0            0  2.84273e-07            0            0\n",
            " -4.4145e-08            0            0            0  1.02439e-07            0            0  2.84273e-07            0\n",
            "           0            0            0            0            0        5e-09            0            0        1e-08]\n",
            "  noise model sigmas:      0.0001      0.0001      0.0001 0.000200785 0.000200785 5.85235e-05 0.000533172 0.000533172      0.0001\n",
            "\n",
            "Factor 6: BetweenFactor(b1,b2)\n",
            "  measured: acc = 0 0 0 gyro = 0 0 0\n",
            "isotropic dim=6 sigma=0.0001\n",
            "\n",
            "\n",
            "Graph 2 (CombinedImuFactor):\n",
            "size: 5\n",
            "\n",
            "Factor 0: PriorFactor on x0\n",
            "  prior mean:  R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t: 0 0 0\n",
            "isotropic dim=6 sigma=0.01\n",
            "\n",
            "Factor 1: PriorFactor on v0\n",
            "  prior mean: [\n",
            "\t0;\n",
            "\t0;\n",
            "\t0\n",
            "]\n",
            "isotropic dim=3 sigma=0.01\n",
            "\n",
            "Factor 2: PriorFactor on b0\n",
            "  prior mean: acc = 0 0 0 gyro = 0 0 0\n",
            "isotropic dim=6 sigma=0.01\n",
            "\n",
            "Factor 3: \n",
            "CombinedImuFactor(x0,v0,x1,v1,b0,b1)\n",
            "  preintegrated measurements:\n",
            "    deltaTij = 1\n",
            "    deltaRij.ypr = ( 0 -0  0)\n",
            "    deltaPij =     0     0 4.905\n",
            "    deltaVij =    0    0 9.81\n",
            "    gyrobias = 0 0 0\n",
            "    acc_bias = 0 0 0\n",
            "\n",
            "  preintMeasCov [    1.285e-08            0            0            0 -1.60129e-08            0            0 -5.26797e-08            0            0            0            0      4.5e-09            0            0\n",
            "           0    1.285e-08            0  1.60129e-08            0            0  5.26797e-08            0            0            0            0            0            0      4.5e-09            0\n",
            "           0            0    1.285e-08            0            0            0            0            0            0            0            0            0            0            0      4.5e-09\n",
            "           0  1.60129e-08            0  4.24698e-08            0            0  1.10381e-07            0            0    1.425e-09            0            0            0   2.6487e-09            0\n",
            "-1.60129e-08            0            0            0  4.24698e-08            0            0  1.10381e-07            0            0    1.425e-09            0  -2.6487e-09            0            0\n",
            "           0            0            0            0            0  3.80833e-09            0            0   6.0125e-09            0            0    1.425e-09            0            0            0\n",
            "           0  5.26797e-08            0  1.10381e-07            0            0  3.14954e-07            0            0      4.5e-09            0            0            0   1.1772e-08            0\n",
            "-5.26797e-08            0            0            0  1.10381e-07            0            0  3.14954e-07            0            0      4.5e-09            0  -1.1772e-08            0            0\n",
            "           0            0            0            0            0   6.0125e-09            0            0    1.285e-08            0            0      4.5e-09            0            0            0\n",
            "           0            0            0    1.425e-09            0            0      4.5e-09            0            0        1e-08            0            0            0            0            0\n",
            "           0            0            0            0    1.425e-09            0            0      4.5e-09            0            0        1e-08            0            0            0            0\n",
            "           0            0            0            0            0    1.425e-09            0            0      4.5e-09            0            0        1e-08            0            0            0\n",
            "     4.5e-09            0            0            0  -2.6487e-09            0            0  -1.1772e-08            0            0            0            0        1e-08            0            0\n",
            "           0      4.5e-09            0   2.6487e-09            0            0   1.1772e-08            0            0            0            0            0            0        1e-08            0\n",
            "           0            0      4.5e-09            0            0            0            0            0            0            0            0            0            0            0        1e-08 ]\n",
            "  noise model: Gaussian [\n",
            "\t21764.5, 0, 0, -0, -12684.3, 0, -0, 7970.24, -0, 0, -1779.09, 0, -3771.17, 0, 0;\n",
            "\t0, 21764.5, 0, 12684.3, 0, 0, -7970.24, -0, -0, 1779.09, 0, 0, 0, -3771.17, 0;\n",
            "\t0, 0, 9611.39, -0, 0, 0, -0, -0, -0, 0, 0, 0, 0, 0, -4325.12;\n",
            "\t0, 0, 0, 16774.4, 0, 0, -5980.2, -0, -0, 300.739, 0, 0, 0, 2596.86, 0;\n",
            "\t0, 0, 0, 0, 16774.4, 0, -0, -5980.2, -0, 0, 300.739, 0, -2596.86, 0, 0;\n",
            "\t0, 0, 0, 0, 0, 32614.6, -0, -0, -16183, 0, 0, 2634.78, 0, 0, 0;\n",
            "\t0, 0, 0, 0, 0, 0, 1828.57, -0, -0, -822.858, 0, 0, 0, -2152.6, 0;\n",
            "\t0, 0, 0, 0, 0, 0, 0, 1828.57, -0, 0, -822.858, 0, 2152.6, 0, 0;\n",
            "\t0, 0, 0, 0, 0, 0, 0, 0, 9611.39, 0, 0, -4325.12, 0, 0, 0;\n",
            "\t0, 0, 0, 0, 0, 0, 0, 0, 0, 10000, 0, 0, 0, -2.18861e-12, 0;\n",
            "\t0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10000, 0, 2.18861e-12, 0, 0;\n",
            "\t0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10000, 0, 0, 0;\n",
            "\t0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10000, 0, 0;\n",
            "\t0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10000, 0;\n",
            "\t0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10000\n",
            "]\n",
            "\n",
            "Factor 4: \n",
            "CombinedImuFactor(x1,v1,x2,v2,b1,b2)\n",
            "  preintegrated measurements:\n",
            "    deltaTij = 1\n",
            "    deltaRij.ypr = ( 0 -0  0)\n",
            "    deltaPij =     0     0 4.905\n",
            "    deltaVij =    0    0 9.81\n",
            "    gyrobias = 0 0 0\n",
            "    acc_bias = 0 0 0\n",
            "\n",
            "  preintMeasCov [    1.285e-08            0            0            0 -1.60129e-08            0            0 -5.26797e-08            0            0            0            0      4.5e-09            0            0\n",
            "           0    1.285e-08            0  1.60129e-08            0            0  5.26797e-08            0            0            0            0            0            0      4.5e-09            0\n",
            "           0            0    1.285e-08            0            0            0            0            0            0            0            0            0            0            0      4.5e-09\n",
            "           0  1.60129e-08            0  4.24698e-08            0            0  1.10381e-07            0            0    1.425e-09            0            0            0   2.6487e-09            0\n",
            "-1.60129e-08            0            0            0  4.24698e-08            0            0  1.10381e-07            0            0    1.425e-09            0  -2.6487e-09            0            0\n",
            "           0            0            0            0            0  3.80833e-09            0            0   6.0125e-09            0            0    1.425e-09            0            0            0\n",
            "           0  5.26797e-08            0  1.10381e-07            0            0  3.14954e-07            0            0      4.5e-09            0            0            0   1.1772e-08            0\n",
            "-5.26797e-08            0            0            0  1.10381e-07            0            0  3.14954e-07            0            0      4.5e-09            0  -1.1772e-08            0            0\n",
            "           0            0            0            0            0   6.0125e-09            0            0    1.285e-08            0            0      4.5e-09            0            0            0\n",
            "           0            0            0    1.425e-09            0            0      4.5e-09            0            0        1e-08            0            0            0            0            0\n",
            "           0            0            0            0    1.425e-09            0            0      4.5e-09            0            0        1e-08            0            0            0            0\n",
            "           0            0            0            0            0    1.425e-09            0            0      4.5e-09            0            0        1e-08            0            0            0\n",
            "     4.5e-09            0            0            0  -2.6487e-09            0            0  -1.1772e-08            0            0            0            0        1e-08            0            0\n",
            "           0      4.5e-09            0   2.6487e-09            0            0   1.1772e-08            0            0            0            0            0            0        1e-08            0\n",
            "           0            0      4.5e-09            0            0            0            0            0            0            0            0            0            0            0        1e-08 ]\n",
            "  noise model: Gaussian [\n",
            "\t21764.5, 0, 0, -0, -12684.3, 0, -0, 7970.24, -0, 0, -1779.09, 0, -3771.17, 0, 0;\n",
            "\t0, 21764.5, 0, 12684.3, 0, 0, -7970.24, -0, -0, 1779.09, 0, 0, 0, -3771.17, 0;\n",
            "\t0, 0, 9611.39, -0, 0, 0, -0, -0, -0, 0, 0, 0, 0, 0, -4325.12;\n",
            "\t0, 0, 0, 16774.4, 0, 0, -5980.2, -0, -0, 300.739, 0, 0, 0, 2596.86, 0;\n",
            "\t0, 0, 0, 0, 16774.4, 0, -0, -5980.2, -0, 0, 300.739, 0, -2596.86, 0, 0;\n",
            "\t0, 0, 0, 0, 0, 32614.6, -0, -0, -16183, 0, 0, 2634.78, 0, 0, 0;\n",
            "\t0, 0, 0, 0, 0, 0, 1828.57, -0, -0, -822.858, 0, 0, 0, -2152.6, 0;\n",
            "\t0, 0, 0, 0, 0, 0, 0, 1828.57, -0, 0, -822.858, 0, 2152.6, 0, 0;\n",
            "\t0, 0, 0, 0, 0, 0, 0, 0, 9611.39, 0, 0, -4325.12, 0, 0, 0;\n",
            "\t0, 0, 0, 0, 0, 0, 0, 0, 0, 10000, 0, 0, 0, -2.18861e-12, 0;\n",
            "\t0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10000, 0, 2.18861e-12, 0, 0;\n",
            "\t0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10000, 0, 0, 0;\n",
            "\t0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10000, 0, 0;\n",
            "\t0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10000, 0;\n",
            "\t0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10000\n",
            "]\n",
            "\n"
          ]
        }
      ],
      "source": [
        "graph_imu = gtsam.NonlinearFactorGraph()\n",
        "graph_combined = gtsam.NonlinearFactorGraph()\n",
        "\n",
        "# Add identical priors to both graphs\n",
        "graph_imu.add(gtsam.PriorFactorPose3(X(0), pose_0, pose_noise))\n",
        "graph_imu.add(gtsam.PriorFactorVector(V(0), vel_0, velocity_noise))\n",
        "graph_imu.add(gtsam.PriorFactorConstantBias(B(0), bias_0, bias_noise))\n",
        "\n",
        "graph_combined.add(gtsam.PriorFactorPose3(X(0), pose_0, pose_noise))\n",
        "graph_combined.add(gtsam.PriorFactorVector(V(0), vel_0, velocity_noise))\n",
        "graph_combined.add(gtsam.PriorFactorConstantBias(B(0), bias_0, bias_noise))\n",
        "\n",
        "# Add factors for two steps (0->1 and 1->2)\n",
        "for k in range(2):\n",
        "    # ImuFactor graph: Add ImuFactor and BetweenFactor separately\n",
        "    graph_imu.add(gtsam.ImuFactor(X(k), V(k), X(k + 1), V(k + 1), B(k), pim_imu))\n",
        "    graph_imu.add(gtsam.BetweenFactorConstantBias(B(k), B(k + 1), gtsam.imuBias.ConstantBias(), bias_walk_noise_model))\n",
        "    \n",
        "    # CombinedImuFactor graph: Add a single combined factor\n",
        "    graph_combined.add(gtsam.CombinedImuFactor(X(k), V(k), X(k+1), V(k+1), B(k), B(k+1), pim_combined))\n",
        "\n",
        "print('Graph 1 (ImuFactor + BetweenFactor):')\n",
        "graph_imu.print('')\n",
        "print('\\nGraph 2 (CombinedImuFactor):')\n",
        "graph_combined.print('')"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "### Step 4: Creating Initial Estimates\n",
        "We need to provide an initial guess for the optimizer. Since the problem is simple (no motion), we can initialize all states and biases to zero. The initial estimates for both graphs will be identical."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 9,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Initial Estimate:\n",
            " Values with 9 values:\n",
            "Value b0: (gtsam::imuBias::ConstantBias)\n",
            "acc = 0 0 0 gyro = 0 0 0\n",
            "\n",
            "Value b1: (gtsam::imuBias::ConstantBias)\n",
            "acc = 0 0 0 gyro = 0 0 0\n",
            "\n",
            "Value b2: (gtsam::imuBias::ConstantBias)\n",
            "acc = 0 0 0 gyro = 0 0 0\n",
            "\n",
            "Value v0: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)\n",
            "[\n",
            "\t0;\n",
            "\t0;\n",
            "\t0\n",
            "]\n",
            "\n",
            "Value v1: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)\n",
            "[\n",
            "\t0;\n",
            "\t0;\n",
            "\t0\n",
            "]\n",
            "\n",
            "Value v2: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)\n",
            "[\n",
            "\t0;\n",
            "\t0;\n",
            "\t0\n",
            "]\n",
            "\n",
            "Value x0: (gtsam::Pose3)\n",
            "R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t: 0 0 0\n",
            "\n",
            "Value x1: (gtsam::Pose3)\n",
            "R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t: 0 0 0\n",
            "\n",
            "Value x2: (gtsam::Pose3)\n",
            "R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t: 0 0 0\n",
            "\n",
            "\n"
          ]
        }
      ],
      "source": [
        "initial_estimate = gtsam.Values()\n",
        "\n",
        "for k in range(3):\n",
        "    initial_estimate.insert(X(k), pose_0)\n",
        "    initial_estimate.insert(V(k), vel_0)\n",
        "    initial_estimate.insert(B(k), bias_0)\n",
        "\n",
        "print('Initial Estimate:\\n', initial_estimate)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "### Step 5: Comparing Factor Jacobians\n",
        "Before optimizing, we can inspect the factors themselves. A nonlinear factor's `linearize` method produces a `JacobianFactor`, which is a linear approximation of the factor's error function around a given state. However, it does mix in the PIM covariances, which we do know are different from Step 2. Hence, here we compare the linearization using numerical derivatives of `evaluateError`, which gives us the *unwhitened* error. \n",
        "\n",
        "Below we show that the combination of Jacobians from `ImuFactor` and `BetweenFactor` for the interval k=0 to k=1 is identical to the single Jacobian from `CombinedImuFactor`, at least for the first 9 rows, which correspond to the NavState error."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 10,
      "metadata": {},
      "outputs": [],
      "source": [
        "imu_factor_01 = gtsam.ImuFactor(X(0), V(0), X(1), V(1), B(0), pim_imu)\n",
        "combined_factor_01 = gtsam.CombinedImuFactor(X(0), V(0), X(1), V(1), B(0), B(1), pim_combined)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "For the first four arguments (really NavState 0 and NavState 1) the Jacobians are indeed identical:"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 11,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Comparing Jacobians w.r.t. pose:\n",
            "[[ 1.     0.     0.     0.     0.     0.   ]       | [[ 1.     0.     0.     0.     0.     0.   ]\n",
            " [ 0.     1.     0.     0.     0.     0.   ]       |  [ 0.     1.     0.     0.     0.     0.   ]\n",
            " [ 0.     0.     1.     0.     0.     0.   ]       |  [ 0.     0.     1.     0.     0.     0.   ]\n",
            " [ 0.     4.905  0.     1.     0.     0.   ]       |  [ 0.     4.905  0.     1.     0.     0.   ]\n",
            " [-4.905  0.     0.     0.     1.     0.   ]       |  [-4.905  0.     0.     0.     1.     0.   ]\n",
            " [ 0.     0.     0.     0.     0.     1.   ]       |  [ 0.     0.     0.     0.     0.     1.   ]\n",
            " [ 0.     9.81   0.     0.     0.     0.   ]       |  [ 0.     9.81   0.     0.     0.     0.   ]\n",
            " [-9.81   0.     0.     0.     0.     0.   ]       |  [-9.81   0.     0.     0.     0.     0.   ]\n",
            " [ 0.     0.     0.     0.     0.     0.   ]]      |  [ 0.     0.     0.     0.     0.     0.   ]\n",
            "                                                   |  [ 0.     0.     0.     0.     0.     0.   ]\n",
            "                                                   |  [ 0.     0.     0.     0.     0.     0.   ]\n",
            "                                                   |  [ 0.     0.     0.     0.     0.     0.   ]\n",
            "                                                   |  [ 0.     0.     0.     0.     0.     0.   ]\n",
            "                                                   |  [ 0.     0.     0.     0.     0.     0.   ]\n",
            "                                                   |  [ 0.     0.     0.     0.     0.     0.   ]]\n"
          ]
        }
      ],
      "source": [
        "from gtsam.utils.numerical_derivative import numericalDerivative51, numericalDerivative61\n",
        "print('Comparing Jacobians w.r.t. pose:')\n",
        "H1 = numericalDerivative51(imu_factor_01.evaluateError, pose_0, vel_0, pose_0, vel_0, bias_0)\n",
        "H2 = numericalDerivative61(combined_factor_01.evaluateError, pose_0, vel_0, pose_0, vel_0, bias_0, bias_0)\n",
        "compare_matrices_side_by_side(H1, H2, precision=3)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 12,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Comparing Jacobians w.r.t. vel:\n",
            "[[0. 0. 0.]                                        | [[0. 0. 0.]\n",
            " [0. 0. 0.]                                        |  [0. 0. 0.]\n",
            " [0. 0. 0.]                                        |  [0. 0. 0.]\n",
            " [1. 0. 0.]                                        |  [1. 0. 0.]\n",
            " [0. 1. 0.]                                        |  [0. 1. 0.]\n",
            " [0. 0. 1.]                                        |  [0. 0. 1.]\n",
            " [1. 0. 0.]                                        |  [1. 0. 0.]\n",
            " [0. 1. 0.]                                        |  [0. 1. 0.]\n",
            " [0. 0. 1.]]                                       |  [0. 0. 1.]\n",
            "                                                   |  [0. 0. 0.]\n",
            "                                                   |  [0. 0. 0.]\n",
            "                                                   |  [0. 0. 0.]\n",
            "                                                   |  [0. 0. 0.]\n",
            "                                                   |  [0. 0. 0.]\n",
            "                                                   |  [0. 0. 0.]]\n"
          ]
        }
      ],
      "source": [
        "from gtsam.utils.numerical_derivative import numericalDerivative52, numericalDerivative62\n",
        "print('Comparing Jacobians w.r.t. vel:')\n",
        "H1 = numericalDerivative52(imu_factor_01.evaluateError, pose_0, vel_0, pose_0, vel_0, bias_0)\n",
        "H2 = numericalDerivative62(combined_factor_01.evaluateError, pose_0, vel_0, pose_0, vel_0, bias_0, bias_0)\n",
        "compare_matrices_side_by_side(H1, H2, precision=3)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 13,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Comparing Jacobians w.r.t. second pose:\n",
            "[[-1.  0.  0.  0.  0.  0.]                         | [[-1.  0.  0.  0.  0.  0.]\n",
            " [ 0. -1.  0.  0.  0.  0.]                         |  [ 0. -1.  0.  0.  0.  0.]\n",
            " [ 0.  0. -1.  0.  0.  0.]                         |  [ 0.  0. -1.  0.  0.  0.]\n",
            " [ 0. -0.  0. -1.  0.  0.]                         |  [ 0. -0.  0. -1.  0.  0.]\n",
            " [ 0.  0.  0.  0. -1.  0.]                         |  [ 0.  0.  0.  0. -1.  0.]\n",
            " [ 0.  0.  0.  0.  0. -1.]                         |  [ 0.  0.  0.  0.  0. -1.]\n",
            " [ 0. -0.  0.  0.  0.  0.]                         |  [ 0. -0.  0.  0.  0.  0.]\n",
            " [ 0.  0.  0.  0.  0.  0.]                         |  [ 0.  0.  0.  0.  0.  0.]\n",
            " [ 0.  0.  0.  0.  0.  0.]]                        |  [ 0.  0.  0.  0.  0.  0.]\n",
            "                                                   |  [ 0.  0.  0.  0.  0.  0.]\n",
            "                                                   |  [ 0.  0.  0.  0.  0.  0.]\n",
            "                                                   |  [ 0.  0.  0.  0.  0.  0.]\n",
            "                                                   |  [ 0.  0.  0.  0.  0.  0.]\n",
            "                                                   |  [ 0.  0.  0.  0.  0.  0.]\n",
            "                                                   |  [ 0.  0.  0.  0.  0.  0.]]\n"
          ]
        }
      ],
      "source": [
        "from gtsam.utils.numerical_derivative import numericalDerivative53, numericalDerivative63\n",
        "print('Comparing Jacobians w.r.t. second pose:')\n",
        "H1 = numericalDerivative53(imu_factor_01.evaluateError, pose_0, vel_0, pose_0, vel_0, bias_0)\n",
        "H2 = numericalDerivative63(combined_factor_01.evaluateError, pose_0, vel_0, pose_0, vel_0, bias_0, bias_0)\n",
        "compare_matrices_side_by_side(H1, H2, precision=3)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 14,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Comparing Jacobians w.r.t. second velocity:\n",
            "[[ 0.  0.  0.]                                     | [[ 0.  0.  0.]\n",
            " [ 0.  0.  0.]                                     |  [ 0.  0.  0.]\n",
            " [ 0.  0.  0.]                                     |  [ 0.  0.  0.]\n",
            " [ 0.  0.  0.]                                     |  [ 0.  0.  0.]\n",
            " [ 0.  0.  0.]                                     |  [ 0.  0.  0.]\n",
            " [ 0.  0.  0.]                                     |  [ 0.  0.  0.]\n",
            " [-1.  0.  0.]                                     |  [-1.  0.  0.]\n",
            " [ 0. -1.  0.]                                     |  [ 0. -1.  0.]\n",
            " [ 0.  0. -1.]]                                    |  [ 0.  0. -1.]\n",
            "                                                   |  [ 0.  0.  0.]\n",
            "                                                   |  [ 0.  0.  0.]\n",
            "                                                   |  [ 0.  0.  0.]\n",
            "                                                   |  [ 0.  0.  0.]\n",
            "                                                   |  [ 0.  0.  0.]\n",
            "                                                   |  [ 0.  0.  0.]]\n"
          ]
        }
      ],
      "source": [
        "from gtsam.utils.numerical_derivative import numericalDerivative54, numericalDerivative64\n",
        "print('Comparing Jacobians w.r.t. second velocity:')\n",
        "H1 = numericalDerivative54(imu_factor_01.evaluateError, pose_0, vel_0, pose_0, vel_0, bias_0)\n",
        "H2 = numericalDerivative64(combined_factor_01.evaluateError, pose_0, vel_0, pose_0, vel_0, bias_0, bias_0)\n",
        "compare_matrices_side_by_side(H1, H2, precision=3)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "For the 5th argument, the initial bias, we see that again the first 9 rows are identical, but obviously the 6 last rows correspond to the bias evolution, and its Jacobian is $I_6$ in the first bias."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 15,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Comparing Jacobians w.r.t. bias B(0):\n",
            "[[ 0.     0.     0.    -1.     0.     0.   ]       | [[ 0.     0.     0.    -1.     0.     0.   ]\n",
            " [ 0.     0.     0.     0.    -1.     0.   ]       |  [ 0.     0.     0.     0.    -1.     0.   ]\n",
            " [ 0.     0.     0.     0.     0.    -1.   ]       |  [ 0.     0.     0.     0.     0.    -1.   ]\n",
            " [-0.5    0.     0.     0.    -1.398  0.   ]       |  [-0.5    0.     0.     0.    -1.398  0.   ]\n",
            " [ 0.    -0.5    0.     1.398  0.     0.   ]       |  [ 0.    -0.5    0.     1.398  0.     0.   ]\n",
            " [ 0.     0.    -0.5    0.     0.     0.   ]       |  [ 0.     0.    -0.5    0.     0.     0.   ]\n",
            " [-1.     0.     0.     0.    -4.414  0.   ]       |  [-1.     0.     0.     0.    -4.414  0.   ]\n",
            " [ 0.    -1.     0.     4.414  0.     0.   ]       |  [ 0.    -1.     0.     4.414  0.     0.   ]\n",
            " [ 0.     0.    -1.     0.     0.     0.   ]]      |  [ 0.     0.    -1.     0.     0.     0.   ]\n",
            "                                                   |  [ 1.     0.     0.     0.     0.     0.   ]\n",
            "                                                   |  [ 0.     1.     0.     0.     0.     0.   ]\n",
            "                                                   |  [ 0.     0.     1.     0.     0.     0.   ]\n",
            "                                                   |  [ 0.     0.     0.     1.     0.     0.   ]\n",
            "                                                   |  [ 0.     0.     0.     0.     1.     0.   ]\n",
            "                                                   |  [ 0.     0.     0.     0.     0.     1.   ]]\n"
          ]
        }
      ],
      "source": [
        "from gtsam.utils.numerical_derivative import numericalDerivative55, numericalDerivative65\n",
        "print('Comparing Jacobians w.r.t. bias B(0):')\n",
        "H1 = numericalDerivative55(imu_factor_01.evaluateError, pose_0, vel_0, pose_0, vel_0, bias_0)\n",
        "H2 = numericalDerivative65(combined_factor_01.evaluateError, pose_0, vel_0, pose_0, vel_0, bias_0, bias_0)\n",
        "compare_matrices_side_by_side(H1, H2, precision=3)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "The derivative in B(1) is negative $I_6$:"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 16,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Note: The IMU factor does not have a B(1) term, so we only show the combined factor:\n",
            "[[ 0.  0.  0.  0.  0.  0.]\n",
            " [ 0.  0.  0.  0.  0.  0.]\n",
            " [ 0.  0.  0.  0.  0.  0.]\n",
            " [ 0.  0.  0.  0.  0.  0.]\n",
            " [ 0.  0.  0.  0.  0.  0.]\n",
            " [ 0.  0.  0.  0.  0.  0.]\n",
            " [ 0.  0.  0.  0.  0.  0.]\n",
            " [ 0.  0.  0.  0.  0.  0.]\n",
            " [ 0.  0.  0.  0.  0.  0.]\n",
            " [-1.  0.  0.  0.  0.  0.]\n",
            " [ 0. -1.  0.  0.  0.  0.]\n",
            " [ 0.  0. -1.  0.  0.  0.]\n",
            " [ 0.  0.  0. -1.  0.  0.]\n",
            " [ 0.  0.  0.  0. -1.  0.]\n",
            " [ 0.  0.  0.  0.  0. -1.]]\n"
          ]
        }
      ],
      "source": [
        "from gtsam.utils.numerical_derivative import numericalDerivative66\n",
        "H2 = numericalDerivative66(combined_factor_01.evaluateError, pose_0, vel_0, pose_0, vel_0, bias_0, bias_0)\n",
        "print(f'Note: The IMU factor does not have a B(1) term, so we only show the combined factor:\\n{H2}')"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "### Step 6: Optimizing and Comparing Final Results\n",
        "Now that we've established the underlying mathematical equivalence of the factors, let's optimize both graphs and compare the final results. They should be identical."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 17,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "--- Results from ImuFactor + BetweenFactor Graph ---\n",
            "Values with 9 values:\n",
            "Value b0: (gtsam::imuBias::ConstantBias)\n",
            "acc =           0           0 1.64997e-23 gyro = 0 0 0\n",
            "\n",
            "Value b1: (gtsam::imuBias::ConstantBias)\n",
            "acc =           0           0 1.65002e-23 gyro = 0 0 0\n",
            "\n",
            "Value b2: (gtsam::imuBias::ConstantBias)\n",
            "acc =           0           0 1.65002e-23 gyro = 0 0 0\n",
            "\n",
            "Value v0: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)\n",
            "[\n",
            "\t0;\n",
            "\t0;\n",
            "\t-1.33561e-23\n",
            "]\n",
            "\n",
            "Value v1: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)\n",
            "[\n",
            "\t0;\n",
            "\t0;\n",
            "\t1.77636e-15\n",
            "]\n",
            "\n",
            "Value v2: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)\n",
            "[\n",
            "\t0;\n",
            "\t0;\n",
            "\t3.55271e-15\n",
            "]\n",
            "\n",
            "Value x0: (gtsam::Pose3)\n",
            "R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t:            0            0 -4.43059e-24\n",
            "\n",
            "Value x1: (gtsam::Pose3)\n",
            "R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t:           0           0 8.88178e-16\n",
            "\n",
            "Value x2: (gtsam::Pose3)\n",
            "R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t:           0           0 3.55271e-15\n",
            "\n",
            "\n",
            "--- Results from CombinedImuFactor Graph ---\n",
            "Values with 9 values:\n",
            "Value b0: (gtsam::imuBias::ConstantBias)\n",
            "acc =           0           0 1.66259e-23 gyro = 0 0 0\n",
            "\n",
            "Value b1: (gtsam::imuBias::ConstantBias)\n",
            "acc =          0          0 1.6626e-23 gyro = 0 0 0\n",
            "\n",
            "Value b2: (gtsam::imuBias::ConstantBias)\n",
            "acc =           0           0 1.66258e-23 gyro = 0 0 0\n",
            "\n",
            "Value v0: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)\n",
            "[\n",
            "\t0;\n",
            "\t0;\n",
            "\t-1.34853e-23\n",
            "]\n",
            "\n",
            "Value v1: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)\n",
            "[\n",
            "\t0;\n",
            "\t0;\n",
            "\t1.77636e-15\n",
            "]\n",
            "\n",
            "Value v2: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)\n",
            "[\n",
            "\t0;\n",
            "\t0;\n",
            "\t3.55271e-15\n",
            "]\n",
            "\n",
            "Value x0: (gtsam::Pose3)\n",
            "R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t:            0            0 -4.51336e-24\n",
            "\n",
            "Value x1: (gtsam::Pose3)\n",
            "R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t:           0           0 8.88178e-16\n",
            "\n",
            "Value x2: (gtsam::Pose3)\n",
            "R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t:           0           0 3.55271e-15\n",
            "\n",
            "\n",
            "Success! The final results from both optimizers are equal.\n"
          ]
        }
      ],
      "source": [
        "# Optimize both graphs\n",
        "optimizer_imu = gtsam.LevenbergMarquardtOptimizer(graph_imu, initial_estimate)\n",
        "result_imu = optimizer_imu.optimize()\n",
        "\n",
        "optimizer_combined = gtsam.LevenbergMarquardtOptimizer(graph_combined, initial_estimate)\n",
        "result_combined = optimizer_combined.optimize()\n",
        "\n",
        "print('--- Results from ImuFactor + BetweenFactor Graph ---')\n",
        "result_imu.print('')\n",
        "print('\\n--- Results from CombinedImuFactor Graph ---')\n",
        "result_combined.print('')\n",
        "\n",
        "# Verify the results are equal\n",
        "assert(result_imu.equals(result_combined, 1e-6))\n",
        "print('\\nSuccess! The final results from both optimizers are equal.')"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "Finally, we can check the posterior marginal covariances on the bias estimates. As expected, these are also identical."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 18,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Covariance for Bias B(0):\n",
            "[[100.   0.   0.   0.  -0.   0.]                   | [[100.   0.   0.   0.  -0.   0.]\n",
            " [  0. 100.   0.  -0.   0.   0.]                   |  [  0. 100.   0.   0.   0.   0.]\n",
            " [  0.   0. 100.   0.   0.   0.]                   |  [  0.   0. 100.   0.   0.   0.]\n",
            " [  0.  -0.   0. 100.   0.   0.]                   |  [  0.   0.   0. 100.   0.   0.]\n",
            " [ -0.   0.   0.   0. 100.   0.]                   |  [ -0.   0.   0.   0. 100.   0.]\n",
            " [  0.   0.   0.   0.   0. 100.]]                  |  [  0.   0.   0.   0.   0. 100.]]\n",
            "\n",
            "Covariance for Bias B(1):\n",
            "[[100.01   0.     0.     0.    -0.     0.  ]       | [[100.01   0.     0.     0.    -0.     0.  ]\n",
            " [  0.   100.01   0.     0.     0.     0.  ]       |  [  0.   100.01   0.     0.     0.     0.  ]\n",
            " [  0.     0.   100.01   0.     0.     0.  ]       |  [  0.     0.   100.01   0.     0.     0.  ]\n",
            " [  0.     0.     0.   100.01   0.     0.  ]       |  [  0.     0.     0.   100.01   0.     0.  ]\n",
            " [ -0.     0.     0.     0.   100.01   0.  ]       |  [ -0.     0.     0.     0.   100.01   0.  ]\n",
            " [  0.     0.     0.     0.     0.   100.01]]      |  [  0.     0.     0.     0.     0.   100.01]]\n",
            "\n",
            "Covariance for Bias B(2):\n",
            "[[100.02   0.     0.     0.    -0.     0.  ]       | [[100.02   0.     0.     0.    -0.     0.  ]\n",
            " [  0.   100.02   0.     0.     0.     0.  ]       |  [  0.   100.02   0.     0.     0.     0.  ]\n",
            " [  0.     0.   100.02   0.     0.     0.  ]       |  [  0.     0.   100.02   0.     0.     0.  ]\n",
            " [  0.     0.     0.   100.02   0.     0.  ]       |  [  0.     0.     0.   100.02   0.     0.  ]\n",
            " [ -0.     0.     0.     0.   100.02   0.  ]       |  [ -0.     0.     0.     0.   100.02   0.  ]\n",
            " [  0.     0.     0.     0.     0.   100.02]]      |  [  0.     0.     0.     0.     0.   100.02]]\n"
          ]
        }
      ],
      "source": [
        "# Analyze marginal covariances\n",
        "marginals_imu = gtsam.Marginals(graph_imu, result_imu)\n",
        "marginals_combined = gtsam.Marginals(graph_combined, result_combined)\n",
        "\n",
        "for k in range(3):\n",
        "    cov_imu_k = marginals_imu.marginalCovariance(B(k))\n",
        "    cov_combined_k = marginals_combined.marginalCovariance(B(k))\n",
        "    \n",
        "    print(f'\\nCovariance for Bias B({k}):')\n",
        "    compare_matrices_side_by_side(1e6*cov_imu_k, 1e6*cov_combined_k, precision=3)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 19,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Covariance for Pose X(0):\n",
            "[[100.   0.   0.   0.  -0.   0.]                   | [[100.   0.   0.   0.   0.   0.]\n",
            " [  0. 100.   0.  -0.   0.   0.]                   |  [  0. 100.   0.  -0.   0.   0.]\n",
            " [  0.   0. 100.   0.   0.   0.]                   |  [  0.   0. 100.   0.   0.   0.]\n",
            " [  0.  -0.   0. 100.   0.   0.]                   |  [  0.  -0.   0. 100.   0.   0.]\n",
            " [ -0.   0.   0.   0. 100.   0.]                   |  [  0.   0.   0.   0. 100.   0.]\n",
            " [  0.   0.   0.   0.   0. 100.]]                  |  [  0.   0.   0.   0.   0. 100.]]\n",
            "\n",
            "Covariance for Velocity V(0):\n",
            "[[100.   0.   0.]                                  | [[100.   0.   0.]\n",
            " [  0. 100.   0.]                                  |  [  0. 100.   0.]\n",
            " [  0.   0. 100.]]                                 |  [  0.   0. 100.]]\n",
            "\n",
            "Covariance for Pose X(1):\n",
            "[[ 200.01     0.       0.       0.    -630.306    0.   ] | [[ 200.013    0.       0.       0.    -630.309    0.   ]\n",
            " [   0.     200.01     0.     630.306    0.       0.   ] |  [   0.     200.013    0.     630.309    0.       0.   ]\n",
            " [   0.       0.     200.01     0.       0.       0.   ] |  [   0.       0.     200.013    0.       0.       0.   ]\n",
            " [   0.     630.306    0.    2826.362    0.       0.   ] |  [   0.     630.309    0.    2826.364    0.       0.   ]\n",
            " [-630.306    0.       0.       0.    2826.362    0.   ] |  [-630.309    0.       0.       0.    2826.364    0.   ]\n",
            " [   0.       0.       0.       0.       0.     225.003]] |  [   0.       0.       0.       0.       0.     225.004]]\n",
            "\n",
            "Covariance for Velocity V(1):\n",
            "[[11772.675     0.        0.   ]                   | [[11772.706     0.        0.   ]\n",
            " [    0.    11772.675     0.   ]                   |  [    0.    11772.706     0.   ]\n",
            " [    0.        0.      200.01 ]]                  |  [    0.        0.      200.013]]\n",
            "\n",
            "Covariance for Pose X(2):\n",
            "[[  500.03      0.        0.        0.    -4385.205     0.   ] | [[  500.027     0.        0.        0.    -4385.189     0.   ]\n",
            " [    0.      500.03      0.     4385.205     0.        0.   ] |  [    0.      500.027     0.     4385.189     0.        0.   ]\n",
            " [    0.        0.      500.03      0.        0.        0.   ] |  [    0.        0.      500.027     0.        0.        0.   ]\n",
            " [    0.     4385.205     0.    54074.013     0.        0.   ] |  [    0.     4385.189     0.    54074.128     0.        0.   ]\n",
            " [-4385.205     0.        0.        0.    54074.013     0.   ] |  [-4385.189     0.        0.        0.    54074.128     0.   ]\n",
            " [    0.        0.        0.        0.        0.      900.029]] |  [    0.        0.        0.        0.        0.      900.029]]\n",
            "\n",
            "Covariance for Velocity V(2):\n",
            "[[73738.273     0.        0.   ]                   | [[73738.274     0.        0.   ]\n",
            " [    0.    73738.274     0.   ]                   |  [    0.    73738.274     0.   ]\n",
            " [    0.        0.      500.03 ]]                  |  [    0.        0.      500.027]]\n"
          ]
        }
      ],
      "source": [
        "# Compare marginal covariances for poses and velocities between both graphs\n",
        "for k in range(3):\n",
        "  cov_pose_imu = marginals_imu.marginalCovariance(X(k))\n",
        "  cov_pose_combined = marginals_combined.marginalCovariance(X(k))\n",
        "  print(f'\\nCovariance for Pose X({k}):')\n",
        "  compare_matrices_side_by_side(1e6*cov_pose_imu, 1e6*cov_pose_combined, precision=3)\n",
        "\n",
        "  cov_vel_imu = marginals_imu.marginalCovariance(V(k))\n",
        "  cov_vel_combined = marginals_combined.marginalCovariance(V(k))\n",
        "  print(f'\\nCovariance for Velocity V({k}):')\n",
        "  compare_matrices_side_by_side(1e6*cov_vel_imu, 1e6*cov_vel_combined, precision=3)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "### Conclusion\n",
        "We have successfully demonstrated that `gtsam.CombinedImuFactor` is a mathematically equivalent but more convenient way to model IMU measurements compared to using a separate `gtsam.ImuFactor` and `gtsam.BetweenFactorConstantBias`. We showed this by verifying that:\n",
        "1. Their preintegrated measurement covariances are consistent.\n",
        "2. The linearization (Jacobians) of a `CombinedImuFactor` is identical to the combined Jacobians of an `ImuFactor` and a `BetweenFactor`.\n",
        "3. The final optimized results and posterior covariances from factor graphs built with both methods are identical.\n",
        "\n",
        "The explanation is that, \n",
        "* with the **same continuous-time PSD $Q_c$** both formulations propagate the *same* bias noise: `CombinedImuFactor` does it inside the PIM, whereas the classic `ImuFactor` relies on a `BetweenFactorConstantBias` whose covariance is $\\Sigma = Q_c\\,T$;\n",
        "* consequently their Jacobians, optimized estimates and posterior covariances are identical (up to round-off), as the notebook demonstrates;\n",
        "\n",
        "Final note: because biases evolve much more slowly than pose/velocity, **using the simpler `ImuFactor` + Between-factor is usually sufficient and faster**.  Reach for `CombinedImuFactor` only when you truly need highly accurate bias estimates at every interval."
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}